# voxel_utils.py

"""
体素化工具模块
===============

此模块提供用于体素化过程的辅助函数，包括：
- 计算模型的包围盒。
- 根据包围盒和体素大小计算体素网格维度。
- 将世界坐标转换为体素索引坐标。
"""

import math
# import numpy as np # 如果未来使用NumPy优化这里的计算
# voxel_utils.py -> world_to_voxel_coords
from .constants import EPSILON # <--- 修改为相对导入

def calculate_bounding_box(vertices: list[list[float]]) -> tuple[float, float, float, float, float, float] | None:
    """计算给定顶点列表的包围盒 (Bounding Box)。"""
    if not vertices:
        # print("警告 (calculate_bounding_box): 顶点列表为空。") # 日志由main.py控制
        return None

    min_x = max_x = vertices[0][0]
    min_y = max_y = vertices[0][1]
    min_z = max_z = vertices[0][2]

    for i in range(1, len(vertices)):
        vx, vy, vz = vertices[i]
        min_x, max_x = min(min_x, vx), max(max_x, vx)
        min_y, max_y = min(min_y, vy), max(max_y, vy)
        min_z, max_z = min(min_z, vz), max(max_z, vz)
    
    # print(f"信息 (calculate_bounding_box): 计算得到的包围盒: " # 日志由main.py控制
    #       f"X:[{min_x:.3f}, {max_x:.3f}], Y:[{min_y:.3f}, {max_y:.3f}], Z:[{min_z:.3f}, {max_z:.3f}]")
    return min_x, max_x, min_y, max_y, min_z, max_z


def calculate_grid_dimensions(
    bbox: tuple[float, float, float, float, float, float], 
    voxel_size: float
) -> tuple[int, int, int] | None:
    """根据包围盒和体素大小计算体素网格的维度。"""
    if voxel_size <= EPSILON: # 检查体素大小是否为有效正数
        # print("错误 (calculate_grid_dimensions): 体素大小必须为正数。") # 日志由main.py控制
        return None

    min_x, max_x, min_y, max_y, min_z, max_z = bbox
    span_x = max_x - min_x
    span_y = max_y - min_y
    span_z = max_z - min_z

    # 确保即使跨度很小或为0，维度至少为1
    grid_dim_x = max(1, math.ceil(span_x / voxel_size if voxel_size > EPSILON else span_x / EPSILON))
    grid_dim_y = max(1, math.ceil(span_y / voxel_size if voxel_size > EPSILON else span_y / EPSILON))
    grid_dim_z = max(1, math.ceil(span_z / voxel_size if voxel_size > EPSILON else span_z / EPSILON))
    
    dims = (int(grid_dim_x), int(grid_dim_y), int(grid_dim_z))
    # print(f"信息 (calculate_grid_dimensions): 体素大小={voxel_size:.3f}, 计算得到的网格维度: {dims}") # 日志由main.py控制
    return dims


def world_to_voxel_coords(
    world_pos: tuple[float, float, float] | list[float], 
    grid_origin: tuple[float, float, float] | list[float], 
    voxel_size: float,
    grid_dimensions: tuple[int, int, int] | list[int] # (dim_x, dim_y, dim_z)
) -> tuple[int, int, int] | None:
    """将世界坐标转换为体素网格的整数索引坐标。"""
    if voxel_size <= EPSILON:
        # print("错误 (world_to_voxel_coords): 体素大小必须为正数。") # 日志由main.py控制
        return None

    origin_x, origin_y, origin_z = grid_origin
    world_x, world_y, world_z = world_pos
    dim_x, dim_y, dim_z = grid_dimensions

    relative_x = world_x - origin_x
    relative_y = world_y - origin_y
    relative_z = world_z - origin_z

    # 使用 floor, 体素[i] 覆盖范围 [i*voxel_size, (i+1)*voxel_size) 相对于原点
    # voxel_idx_x = math.floor(relative_x / voxel_size if voxel_size > EPSILON else relative_x / EPSILON)
    # voxel_idx_y = math.floor(relative_y / voxel_size if voxel_size > EPSILON else relative_y / EPSILON)
    # voxel_idx_z = math.floor(relative_z / voxel_size if voxel_size > EPSILON else relative_z / EPSILON)

    # 避免除以过小的voxel_size (虽然上层已检查，但作为防御)
    safe_voxel_size = max(voxel_size, EPSILON)
    voxel_idx_x = math.floor(relative_x / safe_voxel_size)
    voxel_idx_y = math.floor(relative_y / safe_voxel_size)
    voxel_idx_z = math.floor(relative_z / safe_voxel_size)

    # 夹紧到 [0, dim-1]
    clamped_idx_x = max(0, min(int(voxel_idx_x), dim_x - 1))
    clamped_idx_y = max(0, min(int(voxel_idx_y), dim_y - 1))
    clamped_idx_z = max(0, min(int(voxel_idx_z), dim_z - 1))
    
    return clamped_idx_x, clamped_idx_y, clamped_idx_z

if __name__ == "__main__":
    print("--- 开始独立测试 voxel_utils.py ---")
    # (这里的测试代码可以保持或按需修改，确保它们不依赖外部文件，或能正确导入)
    # 1. 测试 calculate_bounding_box
    print("\n--- 测试 calculate_bounding_box ---")
    test_vertices_1 = [[1.0,2.0,3.0], [0.0,5.0,-1.0], [4.0,0.5,1.5]] # X:[0,4], Y:[0.5,5], Z:[-1,3]
    bbox_1 = calculate_bounding_box(test_vertices_1)
    assert bbox_1 == (0.0, 4.0, 0.5, 5.0, -1.0, 3.0), f"BBox1 失败: {bbox_1}"
    print(f"  BBox1: {bbox_1} - 通过")
    assert calculate_bounding_box([]) is None, "空顶点列表BBox应为None - 通过"
    print("--- calculate_bounding_box 测试通过 ---")

    # 2. 测试 calculate_grid_dimensions
    print("\n--- 测试 calculate_grid_dimensions ---")
    if bbox_1:
        # span_x=4, span_y=4.5, span_z=4
        # vs=0.5 -> (8,9,8)
        # vs=1.0 -> (4,5,4)
        # vs=5.0 -> (1,1,1) (因为max(1, ceil(4/5))=1, max(1, ceil(4.5/5))=1)
        dims_vs05 = calculate_grid_dimensions(bbox_1, 0.5)
        assert dims_vs05 == (8,9,8), f"GridDims vs0.5 失败: {dims_vs05}"
        print(f"  GridDims vs0.5: {dims_vs05} - 通过")
        
        flat_bbox = (0,1,0,1,0,0) # Z span = 0
        dims_flat = calculate_grid_dimensions(flat_bbox, 0.1)
        assert dims_flat == (10,10,1), f"GridDims flat Z 失败: {dims_flat}" # Z应为1
        print(f"  GridDims flat Z (0,1,0,1,0,0) vs0.1: {dims_flat} - 通过")
        assert calculate_grid_dimensions(bbox_1, 0.0) is None, "无效voxel_size应返回None - 通过"
    print("--- calculate_grid_dimensions 测试通过 ---")

    # 3. 测试 world_to_voxel_coords
    print("\n--- 测试 world_to_voxel_coords ---")
    origin_w2v = (0.0, 0.0, 0.0)
    vs_w2v = 0.25
    dims_w2v = (8, 10, 12) # X:0-7, Y:0-9, Z:0-11
    # 点 (0.3, 0.8, 1.1) -> rel (0.3,0.8,1.1) -> float_idx (1.2, 3.2, 4.4) -> floor (1,3,4)
    p1_w = (0.3, 0.8, 1.1); expected_v1 = (1,3,4)
    v1 = world_to_voxel_coords(p1_w, origin_w2v, vs_w2v, dims_w2v)
    assert v1 == expected_v1, f"W2V p1 失败: {v1}, 期望 {expected_v1}"
    print(f"  点 {p1_w} -> {v1} - 通过")
    # 点在边界外，应被夹紧
    p2_w = (10.0, 10.0, 10.0) # 远超范围
    # max_idx = (7,9,11)
    # rel (10,10,10) -> float_idx (40,40,40) -> floor (40,40,40) -> clamped (7,9,11)
    expected_v2 = (dims_w2v[0]-1, dims_w2v[1]-1, dims_w2v[2]-1)
    v2 = world_to_voxel_coords(p2_w, origin_w2v, vs_w2v, dims_w2v)
    assert v2 == expected_v2, f"W2V p2 (far over) 失败: {v2}, 期望 {expected_v2}"
    print(f"  点 {p2_w} -> {v2} (夹紧) - 通过")
    
    p3_w = (-1.0, -1.0, -1.0) # 远低于范围
    expected_v3 = (0,0,0)
    v3 = world_to_voxel_coords(p3_w, origin_w2v, vs_w2v, dims_w2v)
    assert v3 == expected_v3, f"W2V p3 (far under) 失败: {v3}, 期望 {expected_v3}"
    print(f"  点 {p3_w} -> {v3} (夹紧) - 通过")
    assert world_to_voxel_coords(p1_w, origin_w2v, 0.0, dims_w2v) is None, "无效voxel_size W2V应为None - 通过"
    print("--- world_to_voxel_coords 测试通过 ---")

    print("--- voxel_utils.py 独立测试结束 ---")